package robot;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;

import lejos.nxt.Battery;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.SensorPortListener;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;

public class SensorController{
	//TouchSensor touch, LightSensor light, UltrasonicSensor ultra, Battery battery
	List<Object> sensors = new ArrayList<Object>();
	TouchSensor touch = new TouchSensor(SensorPort.S1);
	LightSensor light = new LightSensor(SensorPort.S3);
	UltrasonicSensor ultra = new UltrasonicSensor(SensorPort.S4);
	Boolean tf = false;
	Battery battery ;
	int safeDist = 5;
	int touchInt = 200;
	int notTouchInt = 201;
	int ultraInt = 300;
	int fullBattery = 9000;
	int tooDark = 333;
	int robotScreenLocation = 7;
	final int TOUCH_ENCODE = 111111;
	final int ULTRA_ENCODE = 111112;
	final int LIGHT_ENCODE = 111113;
	final int BATTERY_ENCODE = 111114;
	final int ALL_ENCODE = 111110;

	
	public SensorController(){
		
		SensorPort.S1.addSensorPortListener(new SensorPortListener() { 
			// Port 1: Touch Sensor
				@Override
				public void stateChanged(SensorPort arg0, int arg1, int arg2) {
					if (touch.isPressed()){
						//pilot.stop();
						//String str = "ALERT: obstacle ahead, stopping all actions";
						String str = "S1: Touch Sensor Activated";
						LCD.drawString("Touched!", robotScreenLocation, robotScreenLocation);
						OnBoard.getRobot().getPilot().stop();
						OnBoard.getRobot().getPilot().travel(safeDist);
						OnBoard.connection.sendSensorData(touchInt);
					}
					else
					{
						OnBoard.connection.sendSensorData(notTouchInt);
					}
					
				}	
			});
		
		SensorPort.S2.addSensorPortListener(new SensorPortListener(){
			public void stateChanged(SensorPort arg0, int arg1, int arg2)
			{
				if(light.getLightValue()==light.BLACK)
				{
					String str ="Too Dark For Viewing";
					LCD.drawString("Too Dark", robotScreenLocation , robotScreenLocation);
					OnBoard.connection.sendSensorData(tooDark);
				}
			}
		}
		);
		
		SensorPort.S4.addSensorPortListener(new SensorPortListener() { 
			// Port 4: Ultrasonic Sensor
				@Override
				public void stateChanged(SensorPort arg0, int arg1, int arg2) {
					if (ultra.getDistance() ==5){
						//pilot.stop();
						//String str = "ALERT: obstacle ahead, stopping all actions";
						String str = "S4: Ultrasonic Sensor Activated";
						LCD.drawString("Too close: ", 7, 7);
						OnBoard.getRobot().getPilot().stop();
						OnBoard.getRobot().getPilot().travel(-safeDist);
						OnBoard.connection.sendSensorData(ultraInt);
					}
					
				}
				
			});
	}
	
	public boolean tooClose()
	{
		if(ultra.getDistance() <= 5) //if object is 5cm to robot
		{
			tf = true;
		}
		return tf;
	}

	public void getSensorData(String nToString) 
	{
		if(nToString.equals(TOUCH_ENCODE))
				{
					if (touch.isPressed() == true)
					{
						OnBoard.connection.sendIntToBase(touchInt);
					}
					else
					{
						OnBoard.connection.sendSensorData(notTouchInt);
					}
				}
		if(nToString.equals(ULTRA_ENCODE))
		{
			OnBoard.connection.sendSensorData(ultra.getDistance());
		}
		
		if(nToString.equals(LIGHT_ENCODE))
		{
			OnBoard.connection.sendSensorData(light.readValue());
		}
		
		if(nToString.equals(BATTERY_ENCODE))
		{
			OnBoard.connection.sendSensorData(getBatteryLevelPercentage());
		}
		
		if(nToString.equals(ALL_ENCODE))
		{
			if (touch.isPressed() == true)
			{
				OnBoard.connection.sendIntToBase(touchInt);
			}
			else
			{
				OnBoard.connection.sendSensorData(notTouchInt);
			}
			OnBoard.connection.sendSensorData(ultra.getDistance());
			OnBoard.connection.sendSensorData(getBatteryLevelPercentage());
			OnBoard.connection.sendSensorData(light.readValue());
		}
				
		
	}
	
	public int getBatteryLevelPercentage(){
		int level = Battery.getVoltageMilliVolt();
		double currentlevel = level;
		double dpercentage = (currentlevel/fullBattery)*100;
		int percentage = (int) dpercentage;
		return percentage;
	}
}


